#pragma config(Sensor, in1,    leftLine,       sensorLineFollower)
#pragma config(Sensor, in2,    middleLine,     sensorLineFollower)
#pragma config(Sensor, in3,    rightLine,      sensorLineFollower)
#pragma config(Sensor, dgtl1,  rightEncoder,   sensorQuadEncoder)
#pragma config(Sensor, dgtl3,  leftEncoder,    sensorQuadEncoder)
#pragma config(Motor,  port1,           leftMotor,     tmotorVex393, openLoop, reversed)
#pragma config(Motor,  port10,          rightMotor,    tmotorVex393, openLoop)
//*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//

task main()
{
wait1Msec(1500);

	SensorValue[rightEncoder] = 0;
	SensorValue[leftEncoder] = 0;

	//START FIRST LENGTH

	while(SensorValue[middleLine] < 2500 || SensorValue[leftLine] < 2500)
	{
		if(SensorValue[middleLine] > 2500)
		{
			motor[leftMotor] = 80;
			motor[rightMotor] = 80;
			//Move forward
		}

	else if(SensorValue[leftLine] > 2500)
	{
		motor[leftMotor] = 50;
		motor[rightMotor] = 80;
		//Turn slightly right
	}

	else if (SensorValue[rightLine] > 2500)
	{
		motor[leftMotor] = 80;
		motor[rightMotor] = 50;
		//Turn slightly left
	}
 }
 wait1Msec(300);
//FINISH FIRST LENGTH

 motor[leftMotor] = 0; //set motors to 0
motor[rightMotor] = 0;
wait1Msec(1000);
SensorValue[leftEncoder] = 0; //set encoders to 0
SensorValue[rightEncoder] = 0;

//START TURN
while(SensorValue[leftEncoder]> -283 || SensorValue[rightEncoder]< 283)
	{
	motor[rightMotor] = 80;
	motor[leftMotor] = -80;

	if(SensorValue[leftEncoder]< -283)
		{
			motor[leftMotor] = 0;
		}
if(SensorValue[rightEncoder]> 283)
	 {
	motor[rightMotor] = 0;
   }

 }
motor[leftMotor] = 0;
motor[rightMotor] = 0;
wait1Msec(1000);
SensorValue[leftEncoder] = 0;
SensorValue[rightEncoder] = 0;

//START SECOND LENGTH
	while(SensorValue[middleLine] < 2500 || SensorValue[rightLine] < 2500)
	{
		if(SensorValue[middleLine] > 2500)
		{
			motor[leftMotor] = 80;
			motor[rightMotor] = 80;
			//Move forward
		}

	else if(SensorValue[leftLine] > 2500)
	{
		motor[leftMotor] = 50;
		motor[rightMotor] = 80;
		//Turn slightly right
	}

	else if (SensorValue[rightLine] > 2500)
	{
		motor[leftMotor] = 80;
		motor[rightMotor] = 50;
		//Turn slightly left
	}
 }
 wait1Msec(220);
//FINISH SECOND LENGTH

motor[leftMotor] = 0; //set motors to 0
motor[rightMotor] = 0;
wait1Msec(1000);
SensorValue[leftEncoder] = 0; //set encoders to 0
SensorValue[rightEncoder] = 0;


//START TURN
while(SensorValue[rightEncoder]> -283 || SensorValue[leftEncoder]< 283)
	{
	motor[leftMotor] = 80;
	motor[rightMotor] = -80;

	if(SensorValue[rightEncoder]< -283)
		{
			motor[rightMotor] = 0;
		}
if(SensorValue[leftEncoder]> 283)
	 {
	motor[leftMotor] = 0;
   }

 }
motor[leftMotor] = 0; //set motors to 0
motor[rightMotor] = 0;
wait1Msec(1000);
SensorValue[leftEncoder] = 0; //set encoders to 0
SensorValue[rightEncoder] = 0;

//START THIRD LENGTH

	while(SensorValue[middleLine] < 2500 || SensorValue[rightLine] < 2500)
	{
		if(SensorValue[middleLine] > 2500)
		{
			motor[leftMotor] = 80;
			motor[rightMotor] = 80;
			//Move forward
		}

	else if(SensorValue[leftLine] > 2500)
	{
		motor[leftMotor] = 50;
		motor[rightMotor] = 80;
		//Turn slightly right
	}

	else if (SensorValue[rightLine] > 2500)
	{
		motor[leftMotor] = 80;
		motor[rightMotor] = 50;
		//Turn slightly left
	}
 }
 wait1Msec(215);
//FINISH THIRD LENGTH

motor[leftMotor] = 0; //set motors to 0
motor[rightMotor] = 0;
wait1Msec(1000);
SensorValue[leftEncoder] = 0; //set encoders to 0
SensorValue[rightEncoder] = 0;


//START TURN
while(SensorValue[rightEncoder]> -283 || SensorValue[leftEncoder]< 283)
	{
	motor[leftMotor] = 80;
	motor[rightMotor] = -80;

	if(SensorValue[rightEncoder]< -283)
		{
			motor[rightMotor] = 0;
		}
if(SensorValue[leftEncoder]> 283)
	 {
	motor[leftMotor] = 0;
   }

 }

motor[leftMotor] = 0;
motor[rightMotor] = 0;
wait1Msec(1000);
SensorValue[leftEncoder] = 0;
SensorValue[rightEncoder] = 0;

//START FOURTH LENGTH
while(SensorValue[rightEncoder]< 70 || SensorValue[leftEncoder]< 70 )		// Creates an infinite loop, since "true" always evaluates to true
	{
		if(SensorValue[rightEncoder] == SensorValue[leftEncoder]) // If rightEncoder has counted the same amount as leftEncoder:
		{
			// Move Forward
			motor[rightMotor] = 80;
			motor[leftMotor]  = 80;
		}
		else if(SensorValue[rightEncoder] > SensorValue[leftEncoder])	// If rightEncoder has counted more encoder counts
		{
			// Turn slightly right
			motor[rightMotor] = 50;
			motor[leftMotor]  = 80;
		}
		else	// Only runs if leftEncoder has counted more encoder counts
		{
			// Turn slightly left
			motor[rightMotor] = 80;
			motor[leftMotor]  = 50;
		}
	}
	motor[leftMotor] = 0;
	motor[rightMotor] = 0;
	wait1Msec(1000);
	SensorValue[rightEncoder] = 0;
	SensorValue[leftEncoder] = 0;


	//RETURN
	//START FIFTH LENGTH
	while(SensorValue[rightEncoder]> -70 || SensorValue[leftEncoder]> -70 )		// Creates an infinite loop, since "true" always evaluates to true
	{
		if(SensorValue[rightEncoder] == SensorValue[leftEncoder]) // If rightEncoder has counted the same amount as leftEncoder:
		{
			// Move Forward
			motor[rightMotor] = -80;
			motor[leftMotor]  = -80;
		}
		else if(SensorValue[rightEncoder] < SensorValue[leftEncoder])	// If rightEncoder has counted more encoder counts
		{
			// Turn slightly right
			motor[rightMotor] = -50;
			motor[leftMotor]  = -80;
		}
		else	// Only runs if leftEncoder has counted more encoder counts
		{
			// Turn slightly left
			motor[rightMotor] = -80;
			motor[leftMotor]  = -50;
		}
	}
	motor[leftMotor] = 0;
	motor[rightMotor] = 0;
	wait1Msec(1000);
	SensorValue[rightEncoder] = 0;
	SensorValue[leftEncoder] = 0;

//START TURN
while(SensorValue[rightEncoder]> -283 || SensorValue[leftEncoder]< 283)
	{
	motor[leftMotor] = 80;
	motor[rightMotor] = -80;

	if(SensorValue[rightEncoder]< -283)
		{
			motor[rightMotor] = 0;
		}
if(SensorValue[leftEncoder]> 283)
	 {
	motor[leftMotor] = 0;
   }

 }

motor[leftMotor] = 0;
motor[rightMotor] = 0;
wait1Msec(1000);
SensorValue[leftEncoder] = 0;
SensorValue[rightEncoder] = 0;

//START SIXTH LENGTH

	while(SensorValue[middleLine] < 2500 || SensorValue[leftLine] < 2500)
	{
		if(SensorValue[middleLine] > 2500)
		{
			motor[leftMotor] = 80;
			motor[rightMotor] = 80;
			//Move forward
		}

	else if(SensorValue[leftLine] > 2500)
	{
		motor[leftMotor] = 50;
		motor[rightMotor] = 80;
		//Turn slightly right
	}

	else if (SensorValue[rightLine] > 2500)
	{
		motor[leftMotor] = 80;
		motor[rightMotor] = 50;
		//Turn slightly left
	}
 }
 wait1Msec(215);
//FINISH SIXTH LENGTH

 motor[leftMotor] = 0; //set motors to 0
motor[rightMotor] = 0;
wait1Msec(1000);
SensorValue[leftEncoder] = 0; //set encoders to 0
SensorValue[rightEncoder] = 0;

//START TURN
while(SensorValue[leftEncoder]> -283 || SensorValue[rightEncoder]< 283)
	{
	motor[rightMotor] = 75;
	motor[leftMotor] = -80;

	if(SensorValue[leftEncoder]< -283)
		{
			motor[leftMotor] = 0;
		}
if(SensorValue[rightEncoder]> 283)
	 {
	motor[rightMotor] = 0;
   }

 }
motor[leftMotor] = 0;
motor[rightMotor] = 0;
wait1Msec(1000);
SensorValue[leftEncoder] = 0;
SensorValue[rightEncoder] = 0;

//START SEVENTH LENGTH

while(SensorValue[middleLine] < 2500 || SensorValue[leftLine] < 2500)
	{
		if(SensorValue[middleLine] > 2500)
		{
			motor[leftMotor] = 80;
			motor[rightMotor] = 80;
			//Move forward
		}

	else if(SensorValue[leftLine] > 2500)
	{
		motor[leftMotor] = 50;
		motor[rightMotor] = 80;
		//Turn slightly right
	}

	else if (SensorValue[rightLine] > 2500)
	{
		motor[leftMotor] = 80;
		motor[rightMotor] = 50;
		//Turn slightly left
	}
 }
 wait1Msec(215);

 //TURN

 while(SensorValue[rightEncoder]> -283 || SensorValue[leftEncoder]< 283)
	{
	motor[leftMotor] = 80;
	motor[rightMotor] = -80;

	if(SensorValue[rightEncoder]< -283)
		{
			motor[rightMotor] = 0;
		}
if(SensorValue[leftEncoder]> 283)
	 {
	motor[leftMotor] = 0;
   }

 }

motor[leftMotor] = 0;
motor[rightMotor] = 0;
wait1Msec(1000);
SensorValue[leftEncoder] = 0;
SensorValue[rightEncoder] = 0;

//START LAST LENGTH
while(SensorValue[rightEncoder]< 1400 || SensorValue[leftEncoder]< 1400 )		// Creates an infinite loop, since "true" always evaluates to true
	{
		if(SensorValue[rightEncoder] == SensorValue[leftEncoder]) // If rightEncoder has counted the same amount as leftEncoder:
		{
			// Move Forward
			motor[rightMotor] = 80;
			motor[leftMotor]  = 80;
		}
		else if(SensorValue[rightEncoder] > SensorValue[leftEncoder])	// If rightEncoder has counted more encoder counts
		{
			// Turn slightly right
			motor[rightMotor] = 50;
			motor[leftMotor]  = 80;
		}
		else	// Only runs if leftEncoder has counted more encoder counts
		{
			// Turn slightly left
			motor[rightMotor] = 80;
			motor[leftMotor]  = 50;
		}
	}
	motor[leftMotor] = 0;
	motor[rightMotor] = 0;
	wait1Msec(1000);


}
//FINISH TASK
